/*
 * Copyright (C) 2009-2010 Andre Schulz, Florian Jung, Sebastian Hartte,
 *						   Daniel Trick, Christan Wojek, Konrad Schindler,
 *						   Jens Ackermann, Michael Goesele
 * Copyright (C) 2008-2009 Christopher Evans <chris.evans@irisys.co.uk>, MSc University of Bristol
 *
 * This file is part of SURFGPU.
 *
 * SURFGPU is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 8
 * SURFGPU is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with SURFGPU.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "surflib.h"
#include "kmeans.h"
#include "utils.h"
#include <ctime>
#include <iostream>
#include "cv.h"
#include "highgui.h"
#include "MatchesToMat.h"

//-------------------------------------------------------
// In order to you use OpenSURF, the following illustrates
// some of the simple tasks you can do.  It takes only 1
// function call to extract described SURF features!
// Define PROCEDURE as:
//  - 1 and supply image path to run on static image
//  - 2 to capture from a webcam
//  - 3 to match find an object in an image (work in progress)
//  - 4 to display moving features (work in progress)
//  - 5 to show matches between static images
#define PROCEDURE 3

//-------------------------------------------------------

int mainImage(void);
int mainVideo(void);
int mainMatch(void);
int mainMotionPoints(void);
int mainStaticMatch(void);
int mainKmeans(void);

//-------------------------------------------------------

int main(void) 
{
  if (PROCEDURE == 1) return mainImage();
  if (PROCEDURE == 2) return mainVideo();
  if (PROCEDURE == 3) return mainMatch();
  if (PROCEDURE == 4) return mainMotionPoints();
  if (PROCEDURE == 5) return mainStaticMatch();
  if (PROCEDURE == 6) return mainKmeans();
}

//-------------------------------------------------------

int mainImage(void)
{
  // Declare Ipoints and other stuff
  IpVec ipts;
  IplImage *img=cvLoadImage("Images/img1.jpg");
  IplImage *img_rgba = cvCreateImage(cvSize(img->width, img->height), IPL_DEPTH_8U, 4);
  // convert the image from 24bpp to 32bpp (GPU expects 32bpp)
  cvCvtColor(img, img_rgba, CV_RGB2RGBA);

  // Detect and describe interest points in the image
  {
    surfDetDes(img_rgba, ipts, false, 3, 4, 2, 0.0004f); 
  }

  std::cout<< "OpenSURF found: " << ipts.size() << " interest points" << std::endl;
  //std::cout<< "OpenSURF took: min/avg/max/stddev " << time_min << "/" << time_avg << "/" << time_max << "/" << stddev
	//		<< std::endl;

  // Draw the detected points
  drawIpoints(img, ipts);

  // Display the result
  showImage(img);

  return 0;
}


//-------------------------------------------------------


int mainVideo(void)
{
  // Initialise capture device
  CvCapture* capture = cvCaptureFromCAM( CV_CAP_ANY );
  if(!capture) error("No Capture");

  // Create a window 
  cvNamedWindow("OpenSURF", CV_WINDOW_AUTOSIZE );

  // Declare Ipoints and other stuff
  IpVec ipts;
  IplImage *img=NULL;

  // Main capture loop
  while( 1 ) 
  {
    // Grab frame from the capture source
    img = cvQueryFrame(capture);

    // Extract surf points
    surfDetDes(img, ipts, true, 3, 4, 2, 0.004f);    

    // Draw the detected points
    drawIpoints(img, ipts);

    // Draw the FPS figure
    drawFPS(img);

    // Display the result
    cvShowImage("OpenSURF", img);

    // If ESC key pressed exit loop
    if( (cvWaitKey(10) & 255) == 27 ) break;
  }

  cvReleaseCapture( &capture );
  cvDestroyWindow( "OpenSURF" );
  return 0;
}


//-------------------------------------------------------


int mainMatch(void)
{
  printf("in functie");
    
  // Initialise capture device
    cv::VideoCapture cap(0); // open the default camera
    if(!cap.isOpened())  // check if we succeeded
    {
      printf("can't connect to cammera\n ");
      return -1;
    }

  printf("camera geopend ");
  // Declare Ipoints and other stuff
  IpPairVec matches;
  IpVec ipts, ref_ipts;
  cv::Mat frame, frame_rgba;
  cv::Mat* SrcPoints = NULL; //has to be NULL on init
  cv::Mat* DstPoints = NULL; //has to be NULL on init
  

  // This is the reference object we wish to find in video frame
  // Replace the line below with IplImage *img = cvLoadImage("Images/object.jpg"); 
  // where object.jpg is the planar object to be located in the video
  IplImage *img = cvLoadImage("Images/object.jpg"); 
  IplImage *img_rgba = cvCreateImage(cvSize(img->width, img->height), IPL_DEPTH_8U, 4);
  cvCvtColor(img, img_rgba, CV_RGB2RGBA);
  if (img == NULL) error("Need to load reference image in order to run matching procedure");

  // Extract reference object Ipoints
  surfDetDes(img_rgba, ref_ipts, false, 3, 4, 3, 0.004f);
  printf("Reffence taken \n ");
  img = NULL;
  // Main capture loop
  while( true ) 
  {
    // Grab frame from the capture source
    cap >> frame;
    cvtColor(frame, frame_rgba, CV_RGB2RGBA);

    // Detect and describe interest points in the frame
    surfDetDesMat(&frame_rgba, ipts, false, 3, 4, 3, 0.004f);
    printf("Frame processed \n ");

    // Fill match vector
    
    getMatches(ipts, ref_ipts, matches);
    MatchesToMat(SrcPoints, DstPoints, matches);
    printf("Frame processedn \n ");

    // If ESC key pressed exit loop
    if( (cvWaitKey(10) & 255) == 27 ) break;
  }

  return 0;
}


//-------------------------------------------------------


int mainMotionPoints(void)
{
  // Initialise capture device
  CvCapture* capture = cvCaptureFromCAM( CV_CAP_ANY );
  if(!capture) error("No Capture");

  // Create a window 
  cvNamedWindow("OpenSURF", CV_WINDOW_AUTOSIZE );

  // Declare Ipoints and other stuff
  IpVec ipts, old_ipts, motion;
  IpPairVec matches;
  IplImage *img;

  // Main capture loop
  while( 1 ) 
  {
    // Grab frame from the capture source
    img = cvQueryFrame(capture);

    // Detect and describe interest points in the image
    old_ipts = ipts;
    surfDetDes(img, ipts, true, 3, 4, 2, 0.0004f);

    // Fill match vector
    getMatches(ipts,old_ipts,matches);
    for (unsigned int i = 0; i < matches.size(); ++i) 
    {
      const float & dx = matches[i].first.dx;
      const float & dy = matches[i].first.dy;
      float speed = sqrt(dx*dx+dy*dy);
      if (speed > 5 && speed < 30) 
        drawIpoint(img, matches[i].first, 3);
    }
        
    // Display the result
    cvShowImage("OpenSURF", img);

    // If ESC key pressed exit loop
    if( (cvWaitKey(10) & 255) == 27 ) break;
  }

  // Release the capture device
  cvReleaseCapture( &capture );
  cvDestroyWindow( "OpenSURF" );
  return 0;
}


//-------------------------------------------------------

int mainStaticMatch()
{
  IplImage *img1, *img2;
  img1 = cvLoadImage("Images/img1.jpg");
  img2 = cvLoadImage("Images/img2.jpg");

  IplImage *img1_rgba, *img2_rgba;
  img1_rgba = cvCreateImage(cvSize(img1->width, img1->height), IPL_DEPTH_8U, 4);
  img2_rgba = cvCreateImage(cvSize(img2->width, img2->height), IPL_DEPTH_8U, 4);
  cvCvtColor(img1, img1_rgba, CV_RGB2RGBA);
  cvCvtColor(img2, img2_rgba, CV_RGB2RGBA);

  IpVec ipts1, ipts2;
  surfDetDes(img1_rgba,ipts1,false,4,4,2,0.0001f);
  surfDetDes(img2_rgba,ipts2,false,4,4,2,0.0001f);

  IpPairVec matches;
  getMatches(ipts1,ipts2,matches);

  for (unsigned int i = 0; i < matches.size(); ++i)
  {
    drawPoint(img1,matches[i].first);
    drawPoint(img2,matches[i].second);
  
    const int & w = img1->width;
    cvLine(img1,cvPoint(matches[i].first.x,matches[i].first.y),cvPoint(matches[i].second.x+w,matches[i].second.y), cvScalar(255,255,255),1);
    cvLine(img2,cvPoint(matches[i].first.x-w,matches[i].first.y),cvPoint(matches[i].second.x,matches[i].second.y), cvScalar(255,255,255),1);
  }

  std::cout << "Matches: " << matches.size() << std::endl;

  cvNamedWindow("1", CV_WINDOW_AUTOSIZE );
  cvNamedWindow("2", CV_WINDOW_AUTOSIZE );
  cvShowImage("1", img1);
  cvShowImage("2", img2);
  cvWaitKey(0);

  return 0;
}

//-------------------------------------------------------

int mainKmeans(void)
{
  IplImage *img = cvLoadImage("Images/img1.jpg");
  IpVec ipts;
  Kmeans km;
  
  // Get Ipoints
  surfDetDes(img,ipts,true,3,4,2,0.0006f);

  for (int repeat = 0; repeat < 10; ++repeat)
  {

    IplImage *img = cvLoadImage("Images/img1.jpg");
    km.Run(&ipts, 5, true);
    drawPoints(img, km.clusters);

    for (unsigned int i = 0; i < ipts.size(); ++i)
    {
      cvLine(img, cvPoint(ipts[i].x,ipts[i].y), cvPoint(km.clusters[ipts[i].clusterIndex].x ,km.clusters[ipts[i].clusterIndex].y),cvScalar(255,255,255));
    }

    showImage(img);
  }

  return 0;
}
